#include "RoadGeom.h"
#include "RoadUtil.h"
#include <assert.h>
#include <math.h>
#include <algorithm>
#include <glm/gtx/euler_angles.hpp>
#undef max
#undef min

namespace rg {

using namespace glm;
bool Setting::leftHanded = false;

// Assumes the road have z-axis as up-vector
static const vec3 sRoadUpVector(0, 0, 1);

template<typename VecType>
static float lengthSqr(const VecType& v)
{
	return glm::dot(v, v);
}

RefPoint RefPoint::compute_offset(float offset) const
{
	//This need to consider coordinate system, left side should be positive
	RefPoint ret = *this;
	if (Setting::leftHanded)
	{
		vec2 offsetP = ret.P + vec2(ret.T.y, -ret.T.x) * offset;
		ret.P = offsetP;
	}
	else 
	{
		vec2 offsetP = ret.P + vec2(-ret.T.y, ret.T.x) * offset;
		ret.P = offsetP;
	}
	return ret;
}
mat3 RefPoint::get_rotation() const
{
	vec3 direction = vec3(T.x, T.y, 0);
	vec3 upvector = vec3(0, 0, 1);
	mat3 rotation;
	rotation[0] = direction;
	rotation[1] = normalize(cross(upvector, direction));
	rotation[2] = upvector;
	return rotation;
}

RefPoint RefPoint::lerp(const RefPoint & pt, float a) const
{
	RefPoint ret;
	ret.P = glm::mix(this->P, pt.P, a);
	ret.T = glm::mix(this->T, pt.T, a);
	ret.S = glm::mix(this->S, pt.S, a);
	ret.E = glm::mix(this->E, pt.E, a);
	return ret;
}

double rg::LaneOffset::compute_width(double _s) const
{
	return a + b * _s + c * _s*_s + d * _s*_s*_s;
}

double rg::LaneWidth::compute_width(double s) const
{
	return a + b*s + c*s*s + d*s*s*s;
}

double rg::Lane::compute_width(double s) const
{
	assert(!widths.empty());
	auto w_it = std::upper_bound(widths.begin(), widths.end(), s,
		[](double _s, const LaneWidth& w) {
			return _s < w.soffset;
		}
	);
	if (w_it != widths.begin())
		w_it--;
	if (s < w_it->soffset)
		s = w_it->soffset;

	return w_it->compute_width(s - w_it->soffset);
}

double rg::Elevation::compute_elevation(double s) const
{
	return a + b * s + c * s*s + d * s*s*s;
}

double rg::ElevationProfile::compute_elevation(double s) const
{
	if (elevations.empty())
		return 0;

	auto w_it = std::upper_bound(elevations.begin(), elevations.end(), s,
		[](double _s, const Elevation& elev) {
		return _s < elev.soffset;
	}
	);
	if (w_it != elevations.begin())
		w_it--;
	if (s < w_it->soffset)
		s = w_it->soffset;

	return w_it->compute_elevation(s - w_it->soffset);
}

Lane& LaneSection::add_left()
{
	leftLanes.resize(leftLanes.size() + 1);
	return leftLanes.back();
}

Lane& rg::LaneSection::add_right()
{
	rightLanes.resize(rightLanes.size() + 1);
	return rightLanes.back();
}

static float pointToLineSegmentDistance(const vec3& a, const vec3 b, const vec3& v)
{
	vec3 ab = b - a;
	vec3 av = v - a;

	if (glm::dot(av, ab) <= 0.0) // Point is lagging behind start of the segment, so perpendicular distance is not viable.
		return glm::length(av);  // Use distance to start of segment instead.

	vec3 bv = v - b;

	if (glm::dot(bv, ab) >= 0.0) // Point is advanced past the end of the segment, so perpendicular distance is not viable.
		return glm::length(bv);  // Use distance to end of the segment instead.

	return glm::length(glm::cross(ab, av)) / glm::length(ab);       // Perpendicular distance of point to segment.
}

double rg::ReferenceLine::length() const
{
	return samples.back().S;
}

RefPoint rg::ReferenceLine::computeRefPointAt(double s, size_t * startIdx) const
{
	auto it = std::lower_bound(samples.begin(), samples.end(), s, 
		[](const RefPoint& samp, double _s) {
			return samp.S < _s;
		}
	);

	size_t idx = it - samples.begin();
	if (idx >= samples.size()) {
		idx = samples.size() - 1;
		s = samples.back().S;
	}

	if (s < samples.front().S)
		s = samples.front().S;
	RefPoint ret;
	if (idx > 0) {

		if (idx > 0) {
			ret = samples[idx - 1].lerp(samples[idx], 1.0f - float(samples[idx].S - s) / float(samples[idx].S - samples[idx - 1].S));
			ret.S = s;
		}
		else
			ret = samples[0];
	}
	else
		ret = samples.front();

	if (startIdx)
		*startIdx = idx;

	
	return ret;
}

size_t rg::ReferenceLine::computeStartIdx(double s) const
{
	auto it = std::lower_bound(samples.begin(), samples.end(), s,
		[](const RefPoint& samp, double _s) {
		return samp.S < _s;
	}
	);

	size_t ret = it - samples.begin();
	return ret < samples.size() ? ret : (samples.size() - 1);
}

bool rg::ReferenceLine::isValid() const
{
	if (samples.size() < 2 || samples.front().P == samples.back().P)
		return false;
	else
		return true;
}

RoadGeometry::RoadGeometry() :
	mCachedBBoxMin(FLT_MAX),
	mCachedBBoxMax(-FLT_MAX)
{
}

void RoadGeometry::Initialize(ReferenceLine& refLine, float sampThreshold)
{
	if (!refLine.isValid())
		return;
#if UE_BUILD_DEBUG
	for (size_t i = 0; i < refLine.samples.size() - 1; ++i) {
		assert(refLine.samples[i].S < refLine.samples[i + 1].S);
	}
#endif

	// Update the elevation values for each ref line sample based on elevation profile
	for (auto& samp : refLine.samples) {
		samp.E = (float)mElevationProfile.compute_elevation(samp.S);
	}

	mSampleThreshold = sampThreshold;
	mRoadRefLine = std::move(refLine);
	mHasValidRoadRefLine = true;
}

RefPoint RoadGeometry::SampleLane(double s, int laneIdx) const
{
	double laneRefDis = 0;
	RefPoint refPt = mRoadRefLine.computeRefPointAt(s);
	s = refPt.S;

	if (!mLaneOffsets.empty()) {
		auto laneOffset_it = std::lower_bound(mLaneOffsets.begin(), mLaneOffsets.end(), s,
			[](const LaneOffset& offset, double s) {
			return offset.s < s;
		}
		);
		if (laneOffset_it == mLaneOffsets.end())
			laneOffset_it--;
		laneRefDis = laneOffset_it == mLaneOffsets.end() ? 0 : laneOffset_it->compute_width(s - laneOffset_it->s);
	}

	assert(!mLaneSections.empty());
	auto section_it = std::lower_bound(mLaneSections.begin(), mLaneSections.end(), s,
		[](const LaneSection& section, double s) {
			return section.s < s;
		}
	);

	assert(!mLaneSections.empty());
	if (section_it == mLaneSections.end())
		section_it--;
	
	const LaneSection* pLaneSection = nullptr;
	size_t idx = section_it - mLaneSections.begin();
	pLaneSection = &mLaneSections[idx];
	const std::vector<Lane>* pLanes = nullptr;
	if (laneIdx < 0) 
		pLanes = &pLaneSection->rightLanes;
	else if (laneIdx > 0)
		pLanes = &pLaneSection->leftLanes;
	else
		pLanes = &pLaneSection->centerLanes;

	int absLaneIdx = laneIdx >= 0 ? laneIdx : -laneIdx;

	double laneWidth = 0;
	for (int i = 0; i < absLaneIdx; ++i) {
		if (i >= pLanes->size())
			break;
		laneWidth += (*pLanes)[i].compute_width(s - pLaneSection->s);
	}

	if (laneIdx > 0)
		laneRefDis += laneWidth;
	else
		laneRefDis -= laneWidth;

	return refPt.compute_offset((float)laneRefDis);
}

std::vector<uint8_t> RoadGeometry::Serialize() const
{
	std::vector<uint8_t> ret;

	TSerialize(ret, mRoadRefLine.samples);
	TSerialize(ret, mLaneOffsets);
	SerializeLaneSections(ret, mLaneSections);

	int sectionSize = mCachedLaneSectionDivLines.size();
	TSerialize(ret, sectionSize);

	for (auto&SectionLaneDivLines : mCachedLaneSectionDivLines)
	{
		int lineCount = SectionLaneDivLines.size();
		TSerialize(ret, lineCount);
		for (auto&laneDivLines : SectionLaneDivLines)
		{
			
			TSerialize(ret, laneDivLines.first);
			auto& refline =  laneDivLines.second;
			TSerialize(ret, refline.isDriving);
			TSerialize(ret, refline.laneID);
			TSerialize(ret, refline.laneMidSamples);
			TSerialize(ret, refline.samples);
			TSerialize(ret, refline.roadMarks);
		}
	}

	SerializeStopLines(ret, mStopLines);
	SerializeCrossWalks(ret, mCrossWalks);

	TSerialize(ret, mOptimizeGoemetry);
	TSerialize(ret, mSampleThreshold);
	TSerialize(ret, mMaxRoadWidth);
	TSerialize(ret, RoadId);
	TSerialize(ret, JunctionId);
	TSerialize(ret, mStartCaps);
	TSerialize(ret, mEndCaps);

	return ret;
}

size_t RoadGeometry::Deserialize(void* pSrc)
{
	uint8_t* pByte = (uint8_t*)pSrc;

	pByte += TDeserialize(pByte, mRoadRefLine.samples);
	pByte += TDeserialize(pByte, mLaneOffsets);
	pByte += DeserializeLaneSections(pByte, mLaneSections);

	int sectionSize = 0;
	pByte += TDeserialize(pByte, sectionSize);
	for (int i = 0; i < sectionSize; i++)
	{
		int lineCount = 0;
		pByte += TDeserialize(pByte, lineCount);
		SectionLaneDivLines sectionLaneDivLines;
		for (int j = 0; j < lineCount; j++)
		{
			int key = 0;
			pByte += TDeserialize(pByte, key);
			LaneDivRefLine& laneDivRefLine = sectionLaneDivLines[key] ;
			pByte += TDeserialize(pByte, laneDivRefLine.isDriving);
			pByte += TDeserialize(pByte, laneDivRefLine.laneID);
			pByte += TDeserialize(pByte, laneDivRefLine.laneMidSamples);
			pByte += TDeserialize(pByte, laneDivRefLine.samples);
			pByte += TDeserialize(pByte, laneDivRefLine.roadMarks);
		}
		mCachedLaneSectionDivLines.push_back(sectionLaneDivLines);
	}


	pByte += DeserializeStopLines(pByte, mStopLines);
	pByte += DeserializeCrossWalks(pByte, mCrossWalks);

	pByte += TDeserialize(pByte, mOptimizeGoemetry);
	pByte += TDeserialize(pByte, mSampleThreshold);
	
	pByte += TDeserialize(pByte, mMaxRoadWidth);
	pByte += TDeserialize(pByte, RoadId);
	pByte += TDeserialize(pByte, JunctionId);
	pByte += TDeserialize(pByte, mStartCaps);
	pByte += TDeserialize(pByte, mEndCaps);

	mHasValidRoadRefLine = mRoadRefLine.isValid();
	if (mHasValidRoadRefLine)
		Finalize();

	return pByte - (uint8_t*)pSrc;
}

bool RoadGeometry::IsValid() const
{
	if (!mRoadRefLine.isValid())
		return false;

	if (mCachedBBoxMin == mCachedBBoxMax)
		return false;

	if (mCachedBBoxMin.x > mCachedBBoxMax.x ||
		mCachedBBoxMin.y > mCachedBBoxMax.y ||
		mCachedBBoxMin.z > mCachedBBoxMax.z)
		return false;
	return true;

}

void ReferenceLine::optimizeRefLineSamples(float threshold, std::vector<RefPoint>& line)
{
    if (line.size() == 0)
        return;
	// Remove the unnecessary samples if the reference distance is less than threshold.
	size_t seg_probe_start = 0;
	size_t seg_probe_cur = 1;
	std::vector<RefPoint> optRefLine;
	optRefLine.reserve(line.size());
	optRefLine.push_back(line.front());
	float curveEpsilon = threshold / 100.0f;
	float maxTessDisSqr = threshold * 30;
	maxTessDisSqr *= maxTessDisSqr;

	while (seg_probe_cur < line.size() - 1) {

		bool shouldRemove = true;
		vec3 p0 = line[seg_probe_start].to_xyz();
		for (size_t seg_i = seg_probe_start + 1; seg_i <= seg_probe_cur; ++seg_i) {
			const RefPoint& refPt = line[seg_i];
			vec3 p1 = refPt.to_xyz();
			vec3 p2 = line[seg_probe_cur + 1].to_xyz();
			float dis = pointToLineSegmentDistance(p0, p2, p1);
			float dis01Sqr = lengthSqr(p2 - p0);

			if (dis > curveEpsilon || dis01Sqr > maxTessDisSqr) {
				shouldRemove = false;
				break;
			}
		}

		if (!shouldRemove) {
			optRefLine.push_back(line[seg_probe_cur]);
			seg_probe_start = seg_probe_cur;
			seg_probe_cur = seg_probe_start + 1;
		}
		else {
			seg_probe_cur++;
		}
	}
	optRefLine.push_back(line.back());

    line.swap(optRefLine);
#if UE_BUILD_DEBUG
	for (size_t i = 0; i < samples.size() - 1; ++i) {
		assert(samples[i].S < samples[i + 1].S);
	}
#endif

}


void ReferenceLine::generateRoadMarkGeom(RoadDrawData& drawData) const
{
	if (this->roadMarks.empty())
		return;

	
	

	std::vector<RoadVertex> tempVerts;
	auto _addVertices = [&tempVerts](const RefPoint& refPt, float w) {
		RoadVertex roadVert;
		roadVert.pos = refPt.compute_offset(-w * 0.5f).to_xyz();
		roadVert.normal = glm::vec3(0, 0, 1);
		roadVert.tangent = glm::vec3(refPt.T, 0);
		tempVerts.push_back(roadVert);

		roadVert.pos = refPt.compute_offset(w * 0.5f).to_xyz();
		roadVert.normal = glm::vec3(0, 0, 1);
		roadVert.tangent = glm::vec3(refPt.T, 0);
		tempVerts.push_back(roadVert);
	};


	std::vector<uint32_t>* yellowGroupIndices = nullptr;
	std::vector<uint32_t>* whiteGroupIndices = nullptr;
	std::vector<uint32_t>* curGroupIndices = nullptr;
	
	//auto& triIndices = drawData.groups.back().indices;

	auto _addRoadMarkSegment = [&tempVerts, &curGroupIndices, &drawData]() {
		if (tempVerts.size() < 3)
			return;
		uint32_t baseVertIdx = (uint32_t)drawData.vertices.size();
		uint32_t quadCnt = (uint32_t)tempVerts.size() / 2 - 1;

		for (const auto& v : tempVerts) 
			drawData.vertices.push_back(v);
		
		for (uint32_t i = 0; i < quadCnt; ++i) {

			curGroupIndices->push_back(baseVertIdx + i * 2 + 0);
			curGroupIndices->push_back(baseVertIdx + i * 2 + 2);
			curGroupIndices->push_back(baseVertIdx + i * 2 + 1);
			
			curGroupIndices->push_back(baseVertIdx + i * 2 + 1);
			curGroupIndices->push_back(baseVertIdx + i * 2 + 2);
			curGroupIndices->push_back(baseVertIdx + i * 2 + 3);
		}

		tempVerts.clear();
	};

	struct RoadMarkSegment
	{
		std::vector<RefPoint> samples;
		float width;
		double broken;
		RoadMarkColor color;
		RoadMarkType type;
	};

	auto _getBrokenLength = [](int markType, int subMarkIdx) {
		if (subMarkIdx == 0) {
			switch (markType) {
			case rg::RoadMarkType::kSolid:
				return (double)FLT_MAX;
			case rg::RoadMarkType::kBroken:
				return 2.0;
			case rg::RoadMarkType::kSolidSolid:
				return (double)FLT_MAX;
			case rg::RoadMarkType::kBrokenSolid:
				return 2.0;
			case rg::RoadMarkType::kBrokenBroken:
				return 2.0;
			case rg::RoadMarkType::kSolidBroken:
				return (double)FLT_MAX;
			}
		}
		else {
			switch (markType) {
			case rg::RoadMarkType::kSolid:
				return (double)0;
			case rg::RoadMarkType::kBroken:
				return (double)0;
			case rg::RoadMarkType::kSolidSolid:
				return (double)FLT_MAX;
			case rg::RoadMarkType::kBrokenSolid:
				return (double)FLT_MAX;
			case rg::RoadMarkType::kBrokenBroken:
				return 2.0;
			case rg::RoadMarkType::kSolidBroken:
				return 2.0;
			}
		}
		return (double)FLT_MAX;
	};

	for (int sub_mark_i = 0; sub_mark_i < 2; ++sub_mark_i) {
		RefPoint lastSamp = this->samples.front();
		double accumLen = lastSamp.S;

		std::vector<RoadMarkSegment> markSegments(1);
		markSegments[0].width = this->roadMarks[0].width;
		markSegments[0].color = RoadMarkColor(this->roadMarks[0].type & RoadMarkColor::kColorMask);
		markSegments[0].type = RoadMarkType(this->roadMarks[0].type & RoadMarkType::kTypeMask);
		markSegments[0].broken = _getBrokenLength(markSegments[0].type, sub_mark_i);

		const RoadMark* pCurRoadMark = &this->roadMarks.front();

		for (size_t samp_i = 0; samp_i < this->samples.size(); ++samp_i) {
			const auto& samp = this->samples[samp_i];
			if (samp.S >= pCurRoadMark->soffset && samp.S <= (pCurRoadMark + 1)->soffset) {
				markSegments.back().samples.push_back(samp);
			}
			else if (samp.S > (pCurRoadMark + 1)->soffset) {
				pCurRoadMark++;
				markSegments.back().samples.push_back(this->computeRefPointAt(pCurRoadMark->soffset));

				markSegments.resize(markSegments.size() + 1);
				markSegments.back().width = pCurRoadMark->width;
				markSegments.back().color = RoadMarkColor(pCurRoadMark->type & RoadMarkColor::kColorMask);
				markSegments.back().type = RoadMarkType(pCurRoadMark->type & RoadMarkType::kTypeMask);
				markSegments.back().samples.push_back(this->computeRefPointAt(pCurRoadMark->soffset));
				markSegments.back().broken = _getBrokenLength(markSegments.back().type, sub_mark_i);
			}
		}

		for (const auto& markSeg : markSegments) {

			if (markSeg.broken == 0)
				break;
			double curLen = 0;
			bool isFilling = true;

			float markWidth = markSeg.width;
			float markTOffset = 0;
			switch (markSeg.type) {
			case rg::RoadMarkType::kSolidSolid:
			case rg::RoadMarkType::kBrokenSolid:
			case rg::RoadMarkType::kBrokenBroken:
			case rg::RoadMarkType::kSolidBroken:
				markWidth /= 3.0f;
				if (sub_mark_i == 0)
					markTOffset = -markWidth;
				else
					markTOffset = markWidth;
				break;
			}

			double brokenLen = markSeg.broken;

			if (markSeg.color == RoadMarkColor::kYellow) {
				if (yellowGroupIndices == nullptr) {
					drawData.groups.resize(drawData.groups.size() + 1);
					drawData.groups.back().type = SubGeomType::kRoadMarkYellow;
					yellowGroupIndices = &drawData.groups.back().indices;
				}
				curGroupIndices = yellowGroupIndices;
			}
			else {
				if (whiteGroupIndices == nullptr) {
					drawData.groups.resize(drawData.groups.size() + 1);
					drawData.groups.back().type = SubGeomType::kRoadMarkWhite;
					whiteGroupIndices = &drawData.groups.back().indices;
				}
				curGroupIndices = whiteGroupIndices;
			}

			for (size_t samp_i = 0; samp_i < markSeg.samples.size();) {
				auto samp = markSeg.samples[samp_i].compute_offset(markTOffset);

				double deltaLen = (samp.S - lastSamp.S);

				if ((curLen + deltaLen) > brokenLen) {
					double remainLen = brokenLen - curLen;
					auto interpSamp = this->computeRefPointAt(accumLen + remainLen);
					interpSamp = interpSamp.compute_offset(markTOffset);

					_addVertices(interpSamp, markWidth);
					if (isFilling)
						_addRoadMarkSegment();

					isFilling = !isFilling;
					lastSamp = interpSamp;

					curLen = 0;
					accumLen += remainLen;
					continue;
				}

				if (isFilling)
					_addVertices(samp, markWidth);

				curLen += deltaLen;
				accumLen += deltaLen;
				lastSamp = samp;
				++samp_i;
			}

			_addRoadMarkSegment();
		}

	}
}

ReferenceLine ReferenceLine::computeOffsetRefLine(float offset) const
{
	ReferenceLine ret;
	ret.samples.reserve(this->samples.size());
	for (auto& samp : this->samples)
		ret.samples.push_back(samp.compute_offset(offset));

	return ret;
}
void RoadGeometry::GenerateLaneLines()
{
	if (mCachedLaneSectionDivLines.size() != 0)//already generated
		return;
	for (size_t i = 0; i < mLaneSections.size(); ++i) {
		const LaneSection* pNextSection = (i == mLaneSections.size() - 1) ? nullptr : &mLaneSections[i + 1];
		AddLaneDivingLines(mLaneSections[i], pNextSection);
	}

	// remove invalid lane reference lines
	for (auto& laneLines : mCachedLaneSectionDivLines) {
		for (SectionLaneDivLines::iterator it = laneLines.begin(); it != laneLines.end(); ) {
			if (it->second.samples.empty())
				laneLines.erase(it++);
			else
				++it;
		}
	}

	// Optimize the lane dividing lanes
	if (mOptimizeGoemetry) {
		for (auto& laneLines : mCachedLaneSectionDivLines) {
            for (auto& laneDivLine : laneLines)
            {
                rg::ReferenceLine::optimizeRefLineSamples(mSampleThreshold, laneDivLine.second.samples);
#ifdef MODULE_STANDALONE
                rg::ReferenceLine::optimizeRefLineSamples(mSampleThreshold, laneDivLine.second.laneMidSamples);
#endif
            }

		}
	}
}
void RoadGeometry::GenerateRoadMesh()
{
	GenerateLaneLines();
	std::vector<ReferenceLine> _refLines;
	std::vector<uint32_t> _vertIndexBases;

	auto _addMeshVertices = [&_vertIndexBases, &_refLines](RoadDrawData& drawData, ReferenceLine& refLine) {
		auto& roadVerts = drawData.vertices;
		uint32_t baseIdx = (uint32_t)roadVerts.size();
		_vertIndexBases.push_back(baseIdx);
		roadVerts.resize(roadVerts.size() + refLine.samples.size());
		for (uint32_t vi = 0; vi < refLine.samples.size(); ++vi) {
			auto& newVert = refLine.samples[vi];
			roadVerts[vi + baseIdx].pos = newVert.to_xyz();
			roadVerts[vi + baseIdx].normal = glm::vec3(0, 0, 1);
			roadVerts[vi + baseIdx].tangent = glm::vec3(newVert.T, 0);
		}
		_refLines.push_back(refLine);
	};

	auto _addGeomGroup = [&_refLines, &_vertIndexBases](RoadDrawData& drawData, int refIdx, SubGeomType type) {
		TriangleIndices* pTriIndices = nullptr;
		drawData.groups.resize(drawData.groups.size() + 1);
		drawData.groups.back().type = type;
		pTriIndices = &drawData.groups.back().indices;

		auto& roadIndices = *pTriIndices;

		const std::vector<RefPoint>& leftVerts = _refLines[refIdx].samples;
		const std::vector<RefPoint>& rightVerts = _refLines[refIdx + 1].samples;

		assert(leftVerts.front().S == rightVerts.front().S);
		assert(leftVerts.back().S == rightVerts.back().S);

		uint32_t lineBaseVert_0 = _vertIndexBases[refIdx];
		uint32_t lineBaseVert_1 = _vertIndexBases[refIdx + 1];

		uint32_t linePtIdx_0 = 0;
		uint32_t linePtIdx_1 = 0;
		bool idxOver_0 = false;
		bool idxOver_1 = false;

		bool all_left = false;
		bool all_right = false;

		while (1) {
			if (idxOver_1 || leftVerts[linePtIdx_0].S < rightVerts[linePtIdx_1].S) {
				roadIndices.push_back(lineBaseVert_1 + linePtIdx_1);
				roadIndices.push_back(lineBaseVert_0 + linePtIdx_0);

				linePtIdx_0++;
				if (linePtIdx_0 == leftVerts.size()) {
					idxOver_0 = true;
					linePtIdx_0--;
					if (leftVerts[linePtIdx_0].S < rightVerts[linePtIdx_1].S) {
						all_right = !idxOver_1;
						break;
					}
						
				}
				roadIndices.push_back(lineBaseVert_0 + linePtIdx_0);
			}
			else {
				roadIndices.push_back(lineBaseVert_1 + linePtIdx_1);
				roadIndices.push_back(lineBaseVert_0 + linePtIdx_0);

				linePtIdx_1++;
				if (linePtIdx_1 == rightVerts.size()) {
					idxOver_1 = true;
					linePtIdx_1--;
					if (leftVerts[linePtIdx_0].S > rightVerts[linePtIdx_1].S) {
						all_left = !idxOver_0;
						break;
					}
				}
				roadIndices.push_back(lineBaseVert_1 + linePtIdx_1);
			}

			if (idxOver_0 && idxOver_1)
				break;
		}

		if (all_left) {
			for (uint32_t i = linePtIdx_0 + 1; i < leftVerts.size(); i++) {
				roadIndices.push_back(lineBaseVert_1 + linePtIdx_1);
				roadIndices.push_back(lineBaseVert_0 + linePtIdx_0);
				roadIndices.push_back(lineBaseVert_0 + i);
			}
		}

		if (all_right) {
			for (uint32_t i = linePtIdx_1 + 1; i < rightVerts.size(); i++) {
				roadIndices.push_back(lineBaseVert_1 + linePtIdx_1);
				roadIndices.push_back(lineBaseVert_0 + linePtIdx_0);
				roadIndices.push_back(lineBaseVert_1 + i);
			}
		}
	};


	std::vector<bool> _isDrivingLane;
	for (auto& laneLines : mCachedLaneSectionDivLines) {

		_refLines.clear();
		_vertIndexBases.clear();
		_isDrivingLane.clear();

		for (auto& divLine : laneLines) {
			if (divLine.first != 0)
				_isDrivingLane.push_back(divLine.second.isDriving);
			_addMeshVertices(mCachedMeshDrawData, divLine.second);
		}

		while (_isDrivingLane.size() < _refLines.size() - 1)
			_isDrivingLane.push_back(false);

		for (size_t i = 0; i < _refLines.size() - 1; ++i) {
			_addGeomGroup(mCachedMeshDrawData, (int)i, _isDrivingLane[i] ? SubGeomType::kRoadBase : SubGeomType::kNonDriving);
		}	
	}
	auto addStitchTriangle = [&_isDrivingLane, this](RefPoint p1, RefPoint p2, RefPoint p3, RefPoint p4, SubGeomType type)
	{
		auto& roadVerts = mCachedMeshDrawData.vertices;
		uint32_t baseIdx = (uint32_t)roadVerts.size();
		roadVerts.resize(roadVerts.size() + 4);
		roadVerts[baseIdx].pos = p1.to_xyz();
		roadVerts[baseIdx].normal = glm::vec3(0, 0, 1);
		roadVerts[baseIdx].tangent = glm::vec3(p1.T, 0);
		roadVerts[baseIdx + 1].pos = p2.to_xyz();
		roadVerts[baseIdx + 1].normal = glm::vec3(0, 0, 1);
		roadVerts[baseIdx + 1].tangent = glm::vec3(p2.T, 0);
		roadVerts[baseIdx + 2].pos = p3.to_xyz();
		roadVerts[baseIdx + 2].normal = glm::vec3(0, 0, 1);
		roadVerts[baseIdx + 2].tangent = glm::vec3(p3.T, 0);
		roadVerts[baseIdx + 3].pos = p4.to_xyz();
		roadVerts[baseIdx + 3].normal = glm::vec3(0, 0, 1);
		roadVerts[baseIdx + 3].tangent = glm::vec3(p4.T, 0);

		TriangleIndices* pTriIndices = nullptr;
		mCachedMeshDrawData.groups.resize(mCachedMeshDrawData.groups.size() + 1);
		mCachedMeshDrawData.groups.back().type = type;
		pTriIndices = &mCachedMeshDrawData.groups.back().indices;

		auto& roadIndices = *pTriIndices;
		roadIndices.push_back(baseIdx);
		roadIndices.push_back(baseIdx + 1);
		roadIndices.push_back(baseIdx + 2);
		roadIndices.push_back(baseIdx + 1);
		roadIndices.push_back(baseIdx + 3);
		roadIndices.push_back(baseIdx + 2);
	};
	auto isDrivingLane = [this](bool start, int laneIndex)
	{
		int size = (int)mLaneSections.size();
		auto section = start ? mLaneSections[0]: mLaneSections[size-1];
		for (auto lane : section.centerLanes)
		{
			if (lane.laneID == laneIndex)
				return lane.is_driving;
		}
		for (auto lane : section.leftLanes)
		{
			if (lane.laneID == laneIndex)
				return lane.is_driving;
		}
		for (auto lane : section.rightLanes)
		{
			if (lane.laneID == laneIndex)
				return lane.is_driving;
		}
		return true;
	};
	
	for (auto& laneCap : mStartCaps)
	{
		int index = laneCap.laneIdx;
		auto p1 = laneCap.lanePoint;
		auto p2 = laneCap.lanePoint2;
		
		auto t= this->mCachedLaneSectionDivLines.front();
		auto& refline1 = t[index];
		auto p3 = refline1.samples.front();
		auto& refline2 = t[index > 0 ? index - 1 : index + 1];
		auto p4 = refline2.samples.front();
		auto type = isDrivingLane(true,index) ? SubGeomType::kRoadBase : SubGeomType::kNonDriving;
		index > 0 ? addStitchTriangle(p1, p2, p3, p4, type): addStitchTriangle(p3, p4, p1, p2, type);
	}
	for (auto& laneCap : mEndCaps)
	{
		int index = laneCap.laneIdx;
		auto p1 = laneCap.lanePoint;
		auto p2 = laneCap.lanePoint2;
		
		auto t = this->mCachedLaneSectionDivLines.back();
		auto& refline1 = t[index];
		auto p3 = refline1.samples.back();
		auto& refline2 = t[index > 0 ? index - 1 : index + 1];
		auto p4 = refline2.samples.back();
		auto type = isDrivingLane(true,index) ? SubGeomType::kRoadBase : SubGeomType::kNonDriving;
		index < 0 ? addStitchTriangle(p1, p2, p3, p4, type) : addStitchTriangle(p3, p4, p1, p2, type);
	}
	

	for (auto& laneLines : mCachedLaneSectionDivLines) {
		for (auto& divLine : laneLines) {
			divLine.second.generateRoadMarkGeom(mCachedMeshDrawData);

		}
	}

	//simple case, generate mesh for two staight line with a threshhold 
	auto _addOutlineGeom = [](RoadDrawData& drawData, vec3 line1Start, vec3 line1End, vec3 line2Start, vec3 line2End, const float threshold, const float lineHeight, SubGeomType type) {
		//change the height, so the line won't be hide by the road

		float x1 = line1Start.x;
		float y1 = line1Start.y;
		float x2 = line1End.x;
		float y2 = line1End.y;
		float x = line2Start.x;
		float y = line2Start.y;
		float switchOrder = (y1 - y2) * x + (x2 - x1) * y + x1 * y2 - x2 * y1;
		if (switchOrder < 0)
		{
			//swap line1 and line2, so the generated meshes will have same direction
			vec3 tmp = line1Start;
			line1Start = line2Start;
			line2Start = tmp;
			tmp = line1End;
			line1End = line2End;
			line2End = tmp;
		}

		//Add more vertices for long line
		float length = glm::length(line1Start - line1End);
		int divide = int(ceil(length / threshold));
		uint32_t vSize = divide + 1;

		auto& roadVerts = drawData.vertices;
		uint32_t baseIdx = (uint32_t)roadVerts.size();
		float deltaX1 = (line1End.x - line1Start.x) / divide;
		float deltaY1 = (line1End.y - line1Start.y) / divide;
		vec3 vertex1 = line1Start;
		vertex1.z += lineHeight;
		float deltaX2 = (line2End.x - line2Start.x) / divide;
		float deltaY2 = (line2End.y - line2Start.y) / divide;
		vec3 vertex2 = line2Start;
		vertex2.z += lineHeight;

		roadVerts.resize(roadVerts.size() + vSize * 2);
		for (uint32_t i = 0; i < vSize - 1; i++)
		{
			roadVerts[baseIdx + i].pos = vertex1;
			vertex1.x += deltaX1;
			vertex1.y += deltaY1;
			roadVerts[baseIdx + i].normal = vec3(0, 0, 1);
			roadVerts[baseIdx + i].tangent = vec3(1, 0, 0);

			roadVerts[baseIdx + vSize + i].pos = vertex2;
			vertex2.x += deltaX2;
			vertex2.y += deltaY2;
			roadVerts[baseIdx + vSize + i].normal = vec3(0, 0, 1);
			roadVerts[baseIdx + vSize + i].tangent = vec3(1, 0, 0);
		}
		roadVerts[baseIdx + vSize - 1].pos = line1End;
		roadVerts[baseIdx + vSize - 1].pos.z += lineHeight;
		roadVerts[baseIdx + vSize - 1].normal = vec3(0, 0, 1);
		roadVerts[baseIdx + vSize - 1].tangent = vec3(1, 0, 0);
		roadVerts[baseIdx + 2 * vSize - 1].pos = line2End;
		roadVerts[baseIdx + 2 * vSize - 1].pos.z += lineHeight;
		roadVerts[baseIdx + 2 * vSize - 1].normal = vec3(0, 0, 1);
		roadVerts[baseIdx + 2 * vSize - 1].tangent = vec3(1, 0, 0);

		//add triangles
		TriangleIndices* pTriIndices = nullptr;
		uint32_t lineBaseVert_0 = baseIdx;
		uint32_t lineBaseVert_1 = baseIdx + vSize;

		drawData.groups.resize(drawData.groups.size() + 1);
		drawData.groups.back().type = type;
		pTriIndices = &drawData.groups.back().indices;
		auto& roadIndices = *pTriIndices;
		for (uint32_t i = 0; i < vSize - 1; i++)
		{
			roadIndices.push_back(lineBaseVert_0 + i);
			roadIndices.push_back(lineBaseVert_1 + i);
			roadIndices.push_back(lineBaseVert_0 + i + 1);

			roadIndices.push_back(lineBaseVert_0 + i + 1);
			roadIndices.push_back(lineBaseVert_1 + i);
			roadIndices.push_back(lineBaseVert_1 + i + 1);
		}
	};
	//in 2D plane, give a U(y) value, caculate the intesect V (x)
	auto _intersectPolygon = [](std::vector<vec3>& polygon, vec3& Vmin, vec3& VMax, float lineU)
	{
		for (int i = 0; i < polygon.size() - 1; i++)
		{
			vec3& start = polygon[i];
			vec3& end = polygon[i + 1];
			if ((lineU > start.y && lineU < end.y) || (lineU > end.y && lineU < start.y))
			{
				if (fabs(start.y - end.y) > 0.01)
				{
					float V = (end.x - start.x) *(lineU - start.y) / (end.y - start.y) + start.x;
					float Z = (end.z - start.z) *(lineU - start.y) / (end.y - start.y) + start.z;
					if (V > VMax.x)
						VMax = vec3(V, lineU, Z);
					if (V < Vmin.x)
						Vmin = vec3(V, lineU, Z);
				}
				else
				{
					if (start.x > VMax.x)
						VMax = vec3(start.x, lineU, start.z);
					if (end.x > VMax.x)
						VMax = vec3(end.x, lineU, end.z);
					if (start.x < Vmin.x)
						Vmin = vec3(start.x, lineU, start.z);
					if (end.x < Vmin.x)
						Vmin = vec3(end.x, lineU, end.z);
				}
			}
		}
	};

	//should be fixed by Depth priority
	const float lineHeight = 0.05f;
	for (auto& stopline : mStopLines)
	{
		RefPoint p = mRoadRefLine.computeRefPointAt(stopline.s);
		mat3 rotation = p.get_rotation();
			
		mat3 localRot = eulerAngleY(stopline.pitch)*eulerAngleZ(-1 * stopline.hdg)*eulerAngleX(stopline.roll);
			
		for (int i = 1; i < stopline.outline.size(); i++)
		{
			//one line for each two points
			vec3 Start = stopline.outline[i - 1];
			vec3 End = stopline.outline[i];

			//get local coordinate in s t space.
			Start = localRot * Start;
			End = localRot * End;
			Start.y -= stopline.t;
			End.y -= stopline.t;

			//now get the coordinate in the road space
			Start = rotation * Start;
			Start = Start + vec3(p.P.x, p.P.y, p.E);
			End = rotation * End;
			End = End + vec3(p.P.x, p.P.y, p.E);


			float deltaW = 0.5f*stopline.width;
			float deltax(0.0), deltay(0.0);
			if (fabs(End.x - Start.x) < FLT_EPSILON)
			{
				deltax = deltaW;
				deltay = 0;
			}
			else
			{
				float a = (End.y - Start.y) / (End.x - Start.x);
				deltay = sqrt(deltaW*deltaW / (a*a + 1));
				deltax = a * deltay;
			}
			//get two line
			vec3 line1Start(Start.x + deltax, Start.y - deltay, Start.z);
			vec3 line1End(End.x + deltax, End.y - deltay, End.z);
			vec3 line2Start(Start.x - deltax, Start.y + deltay, Start.z);
			vec3 line2End(End.x - deltax, End.y + deltay, End.z);
			_addOutlineGeom(mCachedMeshDrawData, line1Start, line1End, line2Start, line2End, stopline.width * 3, lineHeight, SubGeomType::kRoadStopLine);
		}

	}
	for (auto& crosswalk : mCrossWalks)
	{
		RefPoint p = mRoadRefLine.computeRefPointAt(crosswalk.s);
		mat3 rotation = p.get_rotation();
		mat3 localRot = eulerAngleY(crosswalk.pitch)*eulerAngleZ(-1 * crosswalk.hdg)*eulerAngleX(crosswalk.roll);

		float minU(1000.0), maxU(-1000.0);
		for (auto corner : crosswalk.outline)
		{
			if (corner.y > maxU)
				maxU = corner.y;
			if (corner.y < minU)
				minU = corner.y;
		}

		std::vector<vec3> crossWalkLine;
		float lineU = minU + crosswalk.crossWalkGap;
		bool widthStep = true;
		while (lineU < maxU)
		{
			vec3 lineVMin{ 1000, lineU, 0 };
			vec3 lineVMax{ -1000, lineU, 0 };
			_intersectPolygon(crosswalk.outline, lineVMin, lineVMax, lineU);

			vec3 local = localRot*lineVMin; //rotate based one U0 V0
			local.y -= crosswalk.t;
			//now get the coordinate in the road space
			local = rotation * local;
			local = local + vec3(p.P.x, p.P.y, p.E);
			crossWalkLine.push_back(glm::vec3(local.x, local.y, local.z));
			local = localRot*lineVMax; //rotate based one U0 V0
			local.y -= crosswalk.t;
			//now get the coordinate in the road space
			local = rotation * local;
			local = local + vec3(p.P.x, p.P.y, p.E);
			crossWalkLine.push_back(glm::vec3(local.x, local.y, local.z));

			lineU += widthStep ? crosswalk.crossWalkWidth : crosswalk.crossWalkGap;
			widthStep = !widthStep;
		}

		for (int i = 0; i < crossWalkLine.size() - 3; i += 4)
		{
			_addOutlineGeom(mCachedMeshDrawData, crossWalkLine[i], crossWalkLine[i + 1], crossWalkLine[i + 2], crossWalkLine[i + 3], 
				 crosswalk.crossWalkWidth * 3, lineHeight, SubGeomType::kRoadCrossWalk);
		}
	}




}

void RoadGeometry::AddLaneDivingLines(const LaneSection& section, const LaneSection * pNextSection)
{
	SectionLaneDivLines sectionDivLines;
	size_t startIdx = 0;
	RefPoint retPt0 = mRoadRefLine.computeRefPointAt(section.s, &startIdx);

	LaneDivRefLine& centerRefLine = sectionDivLines[0];
	centerRefLine.isDriving = false;
	centerRefLine.laneID = 0;
	centerRefLine.samples.push_back(retPt0);
	for (size_t i = startIdx; i < mRoadRefLine.samples.size(); ++i) {
		const auto& curSamp = mRoadRefLine.samples[i];
		assert(curSamp.S >= section.s);
		if (pNextSection) {
			if (curSamp.S > pNextSection->s) {
				RefPoint endRefPt = mRoadRefLine.computeRefPointAt(pNextSection->s);
				centerRefLine.samples.push_back(endRefPt);
				break;
			}
		}
		centerRefLine.samples.push_back(curSamp);
	}

	// Adjust the center reference line so it becomes the lane reference line
	for (size_t i = 0; i < centerRefLine.samples.size(); ++i) {
		auto& curSamp = centerRefLine.samples[i];
		auto it = std::lower_bound(mLaneOffsets.begin(), mLaneOffsets.end(), curSamp.S,
			[](const LaneOffset& offset, double s) {
				return offset.s < s;
			}
		);
		if (it == mLaneOffsets.end() || curSamp.S < it->s) {
			if (it != mLaneOffsets.begin())
				it--;
		}

		double curOffset = 0;
		if (it != mLaneOffsets.end()) {
			curOffset = it->compute_width(curSamp.S - it->s);
		}
		curSamp = curSamp.compute_offset((float)-curOffset);
	}

	HandleRoadMarks(section, section.centerLanes[0], centerRefLine);

	std::vector<double> prevLaneWidths(centerRefLine.samples.size());
	
	// Left lanes
	for (size_t i = 0; i < centerRefLine.samples.size(); ++i)
		prevLaneWidths[i] = 0.0f;
	float maxLeftLaneWidth = 0;

	for (auto lane_it = section.leftLanes.rbegin(); lane_it != section.leftLanes.rend(); lane_it++) {
		auto& lane = *lane_it;
		LaneDivRefLine newRefLine;
		newRefLine.laneID = lane.laneID;
		newRefLine.isDriving = lane.is_driving;

		for (size_t i = 0; i < centerRefLine.samples.size(); ++i) {
			RefPoint curSamp = centerRefLine.samples[i];

			assert(curSamp.S >= section.s && (!pNextSection || curSamp.S <= pNextSection->s));
			double laneWidth = lane.compute_width(curSamp.S - section.s);
			double laneMidDis = 0;

			if (lane.using_border) {
				laneMidDis = (laneWidth + prevLaneWidths[i]) * 0.5f;
				prevLaneWidths[i] = laneWidth;
			}
			else {
				laneWidth += prevLaneWidths[i];
				laneMidDis = (laneWidth + prevLaneWidths[i]) * 0.5f;
				prevLaneWidths[i] = laneWidth;
			}


			if (std::fabs(laneWidth) > std::fabs(maxLeftLaneWidth))
			{
				maxLeftLaneWidth = laneWidth;
			}
			newRefLine.samples.push_back(curSamp.compute_offset((float)laneWidth));
			newRefLine.laneMidSamples.push_back(curSamp.compute_offset((float)laneMidDis));
		}

		HandleRoadMarks(section, lane, newRefLine);
#if UE_BUILD_DEBUG
		for (size_t i = 0; i < newRefLine.samples.size() - 1; ++i) {
			assert(newRefLine.samples[i].S < newRefLine.samples[i + 1].S);
		}
#endif

		sectionDivLines[newRefLine.laneID] = std::move(newRefLine);
	}

	// Right lanes
	for (size_t i = 0; i < centerRefLine.samples.size(); ++i)
		prevLaneWidths[i] = 0.0f;
	float maxRightLaneWidth = 0;

	for (auto& lane : section.rightLanes) {
		LaneDivRefLine newRefLine;
		newRefLine.laneID = lane.laneID;
		newRefLine.isDriving = lane.is_driving;
		for (size_t i = 0; i < centerRefLine.samples.size(); ++i) {
			RefPoint curSamp = centerRefLine.samples[i];

			assert(curSamp.S >= section.s && (!pNextSection || curSamp.S <= pNextSection->s));
			double laneWidth = lane.compute_width(curSamp.S - section.s);
			double laneMidDis = 0;

			if (lane.using_border) {
				laneWidth = -laneWidth;
				laneMidDis = (laneWidth + prevLaneWidths[i]) * 0.5f;
				prevLaneWidths[i] = laneWidth;
			}
			else {
				laneWidth = -laneWidth;
				laneWidth += prevLaneWidths[i];
				laneMidDis = (laneWidth + prevLaneWidths[i]) * 0.5f;
				prevLaneWidths[i] = laneWidth;
			}

			if (std::fabs(laneWidth) > std::fabs(maxRightLaneWidth))
			{
				maxRightLaneWidth = laneWidth;
			}
			newRefLine.samples.push_back(curSamp.compute_offset((float)laneWidth));
			newRefLine.laneMidSamples.push_back(curSamp.compute_offset((float)laneMidDis));
		}

		HandleRoadMarks(section, lane, newRefLine);
#if UE_BUILD_DEBUG
		for (size_t i = 0; i < newRefLine.samples.size() - 1; ++i) {
			assert(newRefLine.samples[i].S < newRefLine.samples[i + 1].S);
		}
#endif
		sectionDivLines[newRefLine.laneID] = std::move(newRefLine);
	}
	mMaxRoadWidth = fabsf(maxLeftLaneWidth - maxRightLaneWidth);
	mCachedLaneSectionDivLines.push_back(std::move(sectionDivLines));

}

void RoadGeometry::HandleRoadMarks(const LaneSection& section, const Lane& lane, ReferenceLine& refLine)
{
	if (lane.roadMarks.empty())
		return;

	refLine.roadMarks = lane.roadMarks;
	refLine.roadMarks.resize(refLine.roadMarks.size() + 1);
	refLine.roadMarks.back().soffset = FLT_MAX;
	refLine.roadMarks.back().width = 0;
	refLine.roadMarks.back().type = 0;
}

void RoadGeometry::CalcuBBox()
{
    auto& roadVerts = mCachedMeshDrawData.vertices;

    glm::vec3 geomBoxMin(FLT_MAX);
    glm::vec3 geomBoxMax(-FLT_MAX);
    auto bboxIncludePt = [&geomBoxMin, &geomBoxMax](const glm::vec3& pt) {
        geomBoxMax = glm::max(geomBoxMax, pt);
        geomBoxMin = glm::min(geomBoxMin, pt);
    };

    for (const auto& pt : roadVerts)
        bboxIncludePt(pt.pos);

    mCachedBBoxMin = geomBoxMin;
    mCachedBBoxMax = geomBoxMax;
}
void RoadGeometry::Finalize()
{
	if (!mHasValidRoadRefLine)
		return;

	GenerateRoadMesh();
	auto& roadVerts = mCachedMeshDrawData.vertices;

	glm::vec3 geomBoxMin(FLT_MAX);
	glm::vec3 geomBoxMax(-FLT_MAX);
	auto bboxIncludePt = [&geomBoxMin, &geomBoxMax](const glm::vec3& pt) {
		geomBoxMax = glm::max(geomBoxMax, pt);
		geomBoxMin = glm::min(geomBoxMin, pt);
	};

	for (const auto& pt : roadVerts)
		bboxIncludePt(pt.pos);

	mCachedBBoxMin = geomBoxMin;
	mCachedBBoxMax = geomBoxMax;
}

void SerializeLaneSections(std::vector<uint8_t>& byteArray, const std::vector<LaneSection>& laneSections)
{
	TSerialize(byteArray, (uint64_t)laneSections.size());
	for (const auto& section : laneSections) {
		TSerialize(byteArray, section.s);

		TSerialize(byteArray, (uint64_t)section.centerLanes.size());
		for (const auto& lane : section.centerLanes) {
			TSerialize(byteArray, lane.widths);
			TSerialize(byteArray, lane.laneID);
            TSerialize(byteArray, lane.roadMarks);
		}

		TSerialize(byteArray, (uint64_t)section.leftLanes.size());
		for (const auto& lane : section.leftLanes) {
			TSerialize(byteArray, lane.using_border);
			TSerialize(byteArray, lane.is_driving);
			TSerialize(byteArray, lane.widths);
			TSerialize(byteArray, lane.laneID);
            TSerialize(byteArray, lane.roadMarks);
		}

		TSerialize(byteArray, (uint64_t)section.rightLanes.size());
		for (const auto& lane : section.rightLanes) {
			TSerialize(byteArray, lane.using_border);
			TSerialize(byteArray, lane.is_driving);
			TSerialize(byteArray, lane.widths);
			TSerialize(byteArray, lane.laneID);
            TSerialize(byteArray, lane.roadMarks);
		}
	}
}

size_t DeserializeLaneSections(void * pSrcData, std::vector<LaneSection>& laneSections)
{
	uint8_t* pByte = (uint8_t*)pSrcData;

	uint64_t sectionCnt = 0;
	pByte += TDeserialize(pByte, sectionCnt);
	laneSections.resize(sectionCnt);
	for (auto& section : laneSections) {
		pByte += TDeserialize(pByte, section.s);

		uint64_t laneCnt = 0;
		pByte += TDeserialize(pByte, laneCnt);
		section.centerLanes.resize(laneCnt);
		for (auto& lane : section.centerLanes) {
			pByte += TDeserialize(pByte, lane.widths);
			pByte += TDeserialize(pByte, lane.laneID);
            pByte += TDeserialize(pByte, lane.roadMarks);
		}

		laneCnt = 0;
		pByte += TDeserialize(pByte, laneCnt);
		section.leftLanes.resize(laneCnt);
		for (auto& lane : section.leftLanes) {
			pByte += TDeserialize(pByte, lane.using_border);
			pByte += TDeserialize(pByte, lane.is_driving);
			pByte += TDeserialize(pByte, lane.widths);
			pByte += TDeserialize(pByte, lane.laneID);
            pByte += TDeserialize(pByte, lane.roadMarks);
		}

		laneCnt = 0;
		pByte += TDeserialize(pByte, laneCnt);
		section.rightLanes.resize(laneCnt);
		for (auto& lane : section.rightLanes) {
			pByte += TDeserialize(pByte, lane.using_border);
			pByte += TDeserialize(pByte, lane.is_driving);
			pByte += TDeserialize(pByte, lane.widths);
			pByte += TDeserialize(pByte, lane.laneID);
            pByte += TDeserialize(pByte, lane.roadMarks);
		}
	}
	return size_t(pByte - (uint8_t*)pSrcData);
}

void SerializeStopLines(std::vector<uint8_t>& byteArray, const std::vector<StopLine>& stopLines)
{
	TSerialize(byteArray, (uint64_t)stopLines.size());
	for (const auto& stopLine : stopLines) {
		TSerialize(byteArray, stopLine.s);
		TSerialize(byteArray, stopLine.t);
		TSerialize(byteArray, stopLine.pitch);
		TSerialize(byteArray, stopLine.hdg);
		TSerialize(byteArray, stopLine.roll);
		TSerialize(byteArray, stopLine.width);

		TSerialize(byteArray, stopLine.outline);
		TSerialize(byteArray, stopLine.outlineoffset);
	}
}

size_t DeserializeStopLines(void* pSrcData, std::vector<StopLine>& stopLines)
{
	uint8_t* pByte = (uint8_t*)pSrcData;

	uint64_t stopLineCnt = 0;
	pByte += TDeserialize(pByte, stopLineCnt);
	stopLines.resize(stopLineCnt);
	for (auto& stopLine : stopLines) {
		pByte += TDeserialize(pByte, stopLine.s);
		pByte += TDeserialize(pByte, stopLine.t);
		pByte += TDeserialize(pByte, stopLine.pitch);
		pByte += TDeserialize(pByte, stopLine.hdg);
		pByte += TDeserialize(pByte, stopLine.roll);
		pByte += TDeserialize(pByte, stopLine.width);

		pByte += TDeserialize(pByte, stopLine.outline);
		pByte += TDeserialize(pByte, stopLine.outlineoffset);
	}
	return size_t(pByte - (uint8_t*)pSrcData);
}

void SerializeCrossWalks(std::vector<uint8_t>& byteArray, const std::vector<CrossWalk>& stopLines)
{
	TSerialize(byteArray, (uint64_t)stopLines.size());
	for (const auto& stopLine : stopLines) {
		TSerialize(byteArray, stopLine.s);
		TSerialize(byteArray, stopLine.t);
		TSerialize(byteArray, stopLine.pitch);
		TSerialize(byteArray, stopLine.hdg);
		TSerialize(byteArray, stopLine.roll);
		TSerialize(byteArray, stopLine.crossWalkGap);
		TSerialize(byteArray, stopLine.crossWalkWidth);

		TSerialize(byteArray, stopLine.outline);
	}
}

size_t DeserializeCrossWalks(void* pSrcData, std::vector<CrossWalk>& stopLines)
{
	uint8_t* pByte = (uint8_t*)pSrcData;

	uint64_t stopLineCnt = 0;
	pByte += TDeserialize(pByte, stopLineCnt);
	stopLines.resize(stopLineCnt);
	for (auto& stopLine : stopLines) {
		pByte += TDeserialize(pByte, stopLine.s);
		pByte += TDeserialize(pByte, stopLine.t);
		pByte += TDeserialize(pByte, stopLine.pitch);
		pByte += TDeserialize(pByte, stopLine.hdg);
		pByte += TDeserialize(pByte, stopLine.roll);
		pByte += TDeserialize(pByte, stopLine.crossWalkGap);
		pByte += TDeserialize(pByte, stopLine.crossWalkWidth);

		pByte += TDeserialize(pByte, stopLine.outline);
	}
	return size_t(pByte - (uint8_t*)pSrcData);
}

} // namespace rg